using System;
using System.Collections.Generic;
using System.Text;
using System.Collections;

namespace UmlControlLibrary.AStar
{
    public class AStar
    {
        public AStarNode[,] nodeMap;
        public ArrayList openNodes = new ArrayList();
        public List<AStarNode> closedNodes = new List<AStarNode>();

        public AStarNode Goal;

        public AStarNode[,] CreateMap(AStarNode.NodeStatus[,] datas)
        {
            AStarNode[,] map = (AStarNode[,])Array.CreateInstance(typeof(AStarNode), datas.GetLength(0), datas.GetLength(1));
            for (int i = 0; i < datas.GetLength(0); i++)
            {
                for (int j = 0; j < datas.GetLength(1); j++)
                {
                    AStarNode node = new AStarNode();
                    map[i, j] = node;
                    node.Map = map;
                    node.X = i;
                    node.Y = j;
                    node.Status = datas[i, j];
                    if (node.Status == AStarNode.NodeStatus.Goal)
                        Goal = node;
                }
            }
            return map;
        }

        public void CalculateHeuristics()
        {
            for (int i = 0; i < nodeMap.GetLength(0); i++)
                for (int j = 0; j < nodeMap.GetLength(1); j++)
                {
                    // TODO : Corriger l'heuristique pour calcul de la distance au bounding block

                    nodeMap[i, j].H = Math.Abs(Goal.X - i) + Math.Abs(Goal.Y - j); // Manhattan
                    nodeMap[i, j].F = nodeMap[i, j].H;
                }
        }

        public void AddStartNodes()
        {
            for (int i = 0; i < nodeMap.GetLength(0); i++)
                for (int j = 0; j < nodeMap.GetLength(1); j++)
                {
                    if (nodeMap[i, j].Status == AStarNode.NodeStatus.Start)
                        openNodes.Add(nodeMap[i, j]);
                }
            openNodes.Sort();
        }

        public AStar(AStarNode.NodeStatus[,] datas)
        {
            nodeMap = CreateMap(datas);
        }

        public List<AStarNode> Run()
        {
            openNodes.Clear();
            closedNodes.Clear();
            CalculateHeuristics();
            AddStartNodes();

            List<AStarNode> neighbours;
            AStarNode current;
            int currentIdx = 0;

            bool Found = false;

            while ((openNodes.Count>0) && (!Found))
            {
                current = (AStarNode)openNodes[currentIdx];
                openNodes.RemoveAt(currentIdx);
                closedNodes.Add(current);
                if (current.Status == AStarNode.NodeStatus.Goal)
                    Found = true;
                if (!Found)
                {
                    neighbours = current.GetNeighbours();
                    foreach (AStarNode node in neighbours)
                    {
                        int cost = node.GetCost(current);
                        {
                            if (!closedNodes.Contains(node))
                            {
                                int g = current.G + node.GetCost(current);
                                bool containsNode = openNodes.Contains(node);
                                if (!containsNode || (node.G > g) || (node.G == 0))
                                {
                                    node.ParentCandidates.Clear();
                                    node.Parent = current;
                                    node.G = g;
                                    node.F = node.G + node.H;
                                    if (!containsNode)
                                        openNodes.Add(node);
                                    node.walked = true;
                                }
                                else if (node.G == g)
                                {
                                    node.ParentCandidates.Add(node.Parent);
                                    node.ParentCandidates.Add(current);
                                }
                            }
                        }
                    }

                    if (openNodes.Count > 0)
                    {
                        current = (AStarNode)openNodes[0];
                        currentIdx = 0;
                    }

                    for(int i=1; i<openNodes.Count;i++)
                    {
                        if (((AStarNode)openNodes[i]).F < current.F)
                        {
                            current = (AStarNode)openNodes[i];
                            currentIdx = i;
                        }
                    }
//                    openNodes.Sort();
                }
            }

            List<AStarNode> Path = null;
            if (Found)
            {
                Path = new List<AStarNode>();
                current = closedNodes[closedNodes.Count-1];
                current.F = 0;
                AStarNode previous = null;
                do
                {
                    current.selected = true;
                    Path.Insert(0, current);

                    if (current.ParentCandidates.Count > 0)
                    {
                        foreach (AStarNode node in current.ParentCandidates)
                            node.F = current.F + node.GetCost(current, previous);
                        current.ParentCandidates.Sort();
                        current.Parent = (AStarNode)current.ParentCandidates[0];
                    }

                    if (current.Status != AStarNode.NodeStatus.Start)
                    {
                        previous = current;
                        current = current.Parent;
                    }
                    else
                        current = null;

                } while (current != null);
            }
            return Path;
        }

        public static List<AStarNode> TrimPath(List<AStarNode> Path)
        {
            List<AStarNode> result = new List<AStarNode>();
            result.Add(Path[0]);

            for (int i = 1; i < Path.Count - 1; i++)
            {
                if ((Path[i + 1].X - Path[i].X != Path[i].X - Path[i - 1].X)
                    || (Path[i + 1].Y - Path[i].Y != Path[i].Y - Path[i - 1].Y))
                    result.Add(Path[i]);
            }
            result.Add(Path[Path.Count - 1]);
            return result;
        }
    }
}
